
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Mag.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mag.h"

/*-----------------------------------macro------------------------------------*/
#define SAMPLING_RES        10
#define SAMPLING_MIN_LAT   -90
#define SAMPLING_MAX_LAT    90
#define SAMPLING_MIN_LON   -180
#define SAMPLING_MAX_LON    180

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
extern const float declination_table[19][37];
extern const float inclination_table[19][37];
extern const float intensity_table[19][37];

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
bool  Mag_GetField(float lat_deg, float lon_deg, float* intensity, float* declination, float* inclination)
{
    bool valid_input = true;    //输入数据是否合理

    //采样精度（10°）
    int32_t min_lat = (int32_t)((int32_t)(lat_deg / SAMPLING_RES) * SAMPLING_RES);
    int32_t min_lon = (int32_t)((int32_t)(lon_deg / SAMPLING_RES) * SAMPLING_RES);

    //输入经纬度限制在合理范围内
    if (lat_deg <= SAMPLING_MIN_LAT)
    {
        min_lat = (int32_t)(SAMPLING_MIN_LAT);
        valid_input = false;
    }

    if (lat_deg >= SAMPLING_MAX_LAT)
    {
        min_lat = (int32_t)((int32_t)(lat_deg / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES);
        valid_input = false;
    }

    if (lon_deg <= SAMPLING_MIN_LON)
    {
        min_lon = (int32_t)(SAMPLING_MIN_LON);
        valid_input = false;
    }

    if (lon_deg >= SAMPLING_MAX_LON)
    {
        min_lon = (int32_t)((int32_t)(lon_deg / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES);
        valid_input = false;
    }

    //计算下界位置
    uint32_t min_lat_index = (uint32_t)((-(SAMPLING_MIN_LAT)+min_lat) / SAMPLING_RES);
    uint32_t min_lon_index = (uint32_t)((-(SAMPLING_MIN_LON)+min_lon) / SAMPLING_RES);

    //二维插值
    float data_00 = intensity_table[min_lat_index][min_lon_index];
    float data_01 = intensity_table[min_lat_index][min_lon_index + 1];
    float data_11 = intensity_table[min_lat_index + 1][min_lon_index + 1];
    float data_10 = intensity_table[min_lat_index + 1][min_lon_index];
    //计算权重系数
    float ratio_lon_0 = (lon_deg - min_lon) / SAMPLING_RES;
    float ratio_lon_1 = 1 - ratio_lon_0;
    float ratio_lat_0 = (lat_deg - min_lat) / SAMPLING_RES;
    float ratio_lat_1 = 1 - ratio_lat_0;

    *intensity = data_00 * ratio_lon_1 * ratio_lat_1 +
                 data_01 * ratio_lon_1 * ratio_lat_0 +
                 data_10 * ratio_lon_0 * ratio_lat_1 +
                 data_11 * ratio_lon_0 * ratio_lat_0;

    //计算declination

    data_00 = declination_table[min_lat_index][min_lon_index];
    data_01 = declination_table[min_lat_index][min_lon_index + 1];
    data_11 = declination_table[min_lat_index + 1][min_lon_index + 1];
    data_10 = declination_table[min_lat_index + 1][min_lon_index];

    *declination = data_00 * ratio_lon_1 * ratio_lat_1 +
                   data_01 * ratio_lon_1 * ratio_lat_0 +
                   data_10 * ratio_lon_0 * ratio_lat_1 +
                   data_11 * ratio_lon_0 * ratio_lat_0;

    //计算inclination
    data_00 = inclination_table[min_lat_index][min_lon_index];
    data_01 = inclination_table[min_lat_index][min_lon_index + 1];
    data_11 = inclination_table[min_lat_index + 1][min_lon_index + 1];
    data_10 = inclination_table[min_lat_index + 1][min_lon_index];

    *inclination = data_00 * ratio_lon_1 * ratio_lat_1 +
                   data_01 * ratio_lon_1 * ratio_lat_0 +
                   data_10 * ratio_lon_0 * ratio_lat_1 +
                   data_11 * ratio_lon_0 * ratio_lat_0;

    return valid_input;
}

float Mag_GetDeclination(float lat_deg, float lon_deg)
{
    float dec_deg = 0.0f, inc_deg = 0.0f, inten_gauss = 0.0f;

    Mag_GetField(lat_deg, lon_deg, &inten_gauss, &dec_deg, &inc_deg);

    return dec_deg;
}

Vector3f_t Mag_GetEarthField(Location* loc)
{
    float dec_deg = 0.0f, inc_deg = 0.0f, inten_gauss = 0.0f;

    Mag_GetField(loc->lat * 1.0e-7f, loc->lng * 1.0e-7f, &inten_gauss, &dec_deg, &inc_deg);

    Vector3f_t mag_ef;
    vec3_set(&mag_ef, inten_gauss, 0.0f, 0.0f);

    Quat_t quat;
    quat_from_euler(&quat, 0.0f, -inc_deg * DEG_TO_RAD, dec_deg * DEG_TO_RAD);

    quat_invert(&quat);
    Vector3f_t mag;
    quat_inv_vec_rotate(&quat, &mag, &mag_ef);

    return mag;
}

/*------------------------------------test------------------------------------*/


